{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Install drake (if necessary) and set up the path.  \n",
    "try:\n",
    "  import pydrake\n",
    "except ImportError:\n",
    "  !curl -s https://raw.githubusercontent.com/RussTedrake/underactuated/master/scripts/setup/jupyter_setup.py > jupyter_setup.py\n",
    "  from jupyter_setup import setup_drake\n",
    "  setup_drake()\n",
    "# Note: On Google's Colaboratory, this will take a minute, but should only need to reinstall once every 12 hours.\n",
    "# Colab will ask you to \"Reset all runtimes\"; say no to save yourself the reinstall."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## A brief introduction to optimization"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Optimization is finding the best solution to a problem subject to particular constraints. For example, driving the most efficient route from point A to point B given the amount of traffic on the roads. Mathematical optimization is written in a particular structure:\n",
    "\n",
    "\\begin{align*}\n",
    "\\min_{x} \\ \\ & f(x) & \\\\\n",
    "\\text{s.t.} \\ \\ & g_i(x) \\leq 0, & i = 1, ..., m \\\\\n",
    "         & h_j(x) = 0, & j = 1, ..., p \\\\\n",
    "\\end{align*}\n",
    "\n",
    "Here, $m \\geq 0$ and $p \\geq 0$.\n",
    "\n",
    "$f(x)$ is called the **objective function** or **cost**. <br>\n",
    "$g_i(x)$ are the **inequality constraints**. <br>\n",
    "$h_j(x)$ are the **equality constraints**."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "There are a couple of important classes of optimization problems:\n",
    "\n",
    "- Least Squares:\n",
    "    Solving a problem of the form $$\\min \\|{Ax - b}\\|^2_2.$$\n",
    "    There is an analytic solution $$x^* = (A^TA)^{-1}A^Tb.$$\n",
    "    \n",
    "    \n",
    "- Linear Programs (LPs):\n",
    "    Solving a problem of the form \\begin{align*}\n",
    "                                        \\min_{x} \\ & c^Tx, \\\\\n",
    "                                        \\text{s.t.} \\ \\ & Ax \\leq b.\\\\\n",
    "                                    \\end{align*}\n",
    "    LPs have linear object functions and linear constraints.\n",
    "    \n",
    "    \n",
    "- Quadratic Programs (QPs):\n",
    "    Solving a problem of the form \\begin{align*}\n",
    "                                        \\min_{x} \\ & \\frac{1}{2}x^TQx + c^Tx, \\\\\n",
    "                                        \\text{s.t.} \\ \\ &  Ax \\leq b. \\\\\n",
    "                                    \\end{align*}\n",
    "    QPs have quadratic object functions and linear constraints.\n",
    "\n",
    "There are some other important classes, including Semidefinite Programs (SDPs), and (Mixed-)Integer Programs (MIP/IP).  We'll add more about those to these notes soon.  For now, you can check out the [course notes](https://ocw.mit.edu/courses/electrical-engineering-and-computer-science/6-079-introduction-to-convex-optimization-fall-2009/lecture-notes/MIT6_079F09_lec01.pdf) from a version of MIT's 6.079 Intro to Convex Optimization for more information."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "There are a variety of open-source and commercial optimization solvers that are really good at solving many types of optimization problems. Some examples include [SNOPT](https://web.stanford.edu/group/SOL/guides/sndoc7.pdf), [IPOPT](https://projects.coin-or.org/Ipopt), and [Gurobi](http://www.gurobi.com/index)."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## How to use Mathematical Program"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "One area of optimization tools offered in pydrake is the MathematicalProgram interface.  MathematicalProgram is a class that abstracts many different useful types of optimization solvers.  This makes it so from one interface, you can access many different solvers.  The MathematicalProgram component of Drake is comparable to CVX or YALMIP in the MATLAB ecosystem, or JuMP, in the Julia ecosystem. To get a concise overview of which solvers are supported for which different types of optimization problems, check out [this chart](http://drake.mit.edu/doxygen_cxx/group__solvers.html).  \n",
    "\n",
    "Here, we provide enough sample code to familiarize you with the tool and give you a sense of how to solve simple problems. If you're curious to dig deeper, take a look at the [Technical C++ Doxygen Documention](http://drake.mit.edu/doxygen_cxx/classdrake_1_1solvers_1_1_mathematical_program.html), and the [C++ source code](https://github.com/RobotLocomotion/drake/blob/master/solvers/mathematical_program.h).\n",
    "\n",
    "In addition to the code snippets below, these two tips are also very useful:\n",
    "\n",
    "- Once you construct a MathematicalProgram object, i.e. `mp = MathematicalProgram()`, the tab completion in your jupyter notebook can be very helpful.  \n",
    "\n",
    " --> For example, let's say you want to know if a MathematicalProgram can print how many decision variables currently exist.  Tab completing on `mp.` and scrolling through, you'll find `num_vars`.  Indeed `mp.num_vars()` will do the trick.\n",
    " \n",
    " --> Want to know which solver MP is currently using under the hood for a particular problem instance?\n",
    " \n",
    "```python\n",
    "result = Solve(mp) # must first solve the program (which forces a solver to be chosen)\n",
    "solver = result.get_solver_id()\n",
    "solver.name() # name will tab complete after creating a solver object\n",
    "```\n",
    "- An additional resource for how to use MathematicalProgram is the tests written for it. There are a significant amount of tests for MathematicalProgram, written in C++.  See [here](https://github.com/RobotLocomotion/drake/blob/master/solvers/test/mathematical_program_test.cc) but also other tests in that folder.  Occasionally you will still find a C++ feature that does not yet have pydrake bindings -- for those familiar with pybind, the bindings for MathematicalProgram are generated [here](https://github.com/RobotLocomotion/drake/blob/master/bindings/pydrake/solvers/mathematicalprogram_py.cc), and are demonstrated in numerous tests [here](https://github.com/RobotLocomotion/drake/tree/master/bindings/pydrake/solvers/test). Obviously though the features  demonstrated below all have pydrake bindings"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Okay, but how do I actually do an optimization problem?  \n",
    "\n",
    "How do we translate something written on the board as a linear program, and write it down in code?\n",
    "\n",
    "Here is a very simple example of a linear optimization problem:\n",
    "\n",
    "\\begin{align*}\n",
    "        \\min_{x} \\ & x \\\\\n",
    "        \\text{s.t.} \\ \\ & x \\ge 1 \\\\\n",
    "\\end{align*}\n",
    "\n",
    "And the corresponding Mathematical Program code is below."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "from pydrake.solvers.mathematicalprogram import MathematicalProgram, Solve\n",
    "import numpy as np\n",
    "import math"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "True\n",
      "[ 1.]\n"
     ]
    }
   ],
   "source": [
    "mp = MathematicalProgram()\n",
    "x = mp.NewContinuousVariables(1, \"x\")\n",
    "mp.AddLinearCost(x[0]*1.0)\n",
    "mp.AddLinearConstraint(x[0] >= 1)\n",
    "result = Solve(mp)\n",
    "print(result.is_success())\n",
    "print(result.GetSolution(x))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Note that written down slightly incorrectly, you will not get the answer you were looking for.  What is wrong about the two examples below?"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "False\n",
      "SolutionResult.kUnbounded\n"
     ]
    }
   ],
   "source": [
    "mp = MathematicalProgram()\n",
    "x = mp.NewContinuousVariables(1, \"x\")\n",
    "mp.AddLinearCost(x[0]*1.0)\n",
    "result = Solve(mp)\n",
    "print(result.is_success())\n",
    "print(result.get_solution_result())"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Note that you can get some information about the solution by printing out the result of `Solve(mp)`, which returns a [MathematicalProgramResult](https://drake.mit.edu/doxygen_cxx/classdrake_1_1solvers_1_1_mathematical_program_result.html)."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "False\n",
      "SolutionResult.kUnbounded\n"
     ]
    }
   ],
   "source": [
    "mp = MathematicalProgram()\n",
    "x = mp.NewContinuousVariables(1, \"x\")\n",
    "mp.AddLinearCost(x[0]*1.0)\n",
    "mp.AddLinearConstraint(x[0] <= 1)\n",
    "result = Solve(mp)\n",
    "print(result.is_success())\n",
    "print(result.get_solution_result())"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Here's a slightly more complicated example."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[ 1.23623682]\n"
     ]
    }
   ],
   "source": [
    "mp = MathematicalProgram()\n",
    "alpha = mp.NewContinuousVariables(1, \"alpha\")\n",
    "mp.AddLinearCost(alpha[0]*1.0)\n",
    "for xi in np.arange(-5*np.pi, 5*np.pi+np.pi/8, np.pi/8):\n",
    "    mp.AddLinearConstraint(alpha[0] - math.cos(xi)**2 + math.sin(xi) >= 0)\n",
    "    \n",
    "result = Solve(mp)\n",
    "print(result.GetSolution(alpha))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Note the MathematicalProgram is formulated in terms of \"costs\", and will minimize the objective function's costs when calling `Solve(mp)`.  How can we maximize functions? Just add a negative sign:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[ 4.]\n"
     ]
    }
   ],
   "source": [
    "mp = MathematicalProgram()\n",
    "x = mp.NewContinuousVariables(1, \"x\")\n",
    "mp.AddLinearCost(-x[0]*1.0)\n",
    "mp.AddLinearConstraint(x[0] <= 4)\n",
    "result = Solve(mp)\n",
    "print(result.GetSolution(x))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "If, instead, we prefer quadratic cost functions, then this yields a quadratic program."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[ 3.]\n"
     ]
    }
   ],
   "source": [
    "mp = MathematicalProgram()\n",
    "x = mp.NewContinuousVariables(1, \"x\")\n",
    "mp.AddQuadraticCost((x[0]-3)**2)\n",
    "result = Solve(mp)\n",
    "print(result.GetSolution(x))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Note that as above, problems with quadratic costs can be well formulated even without any constraints, which is not true of problems with only linear constraints.  And note that, because we didn't have any constraints, drake used a special purpose \"solver\", which in this case was just the closed-form solution."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Note that although there is no `QuadraticConstraint` in MathematicalProgram, it can generally handle a quadratic constraint, and many other different types of constraints, through `AddConstraint`, where inside the argument to the function is a symbolic formula of type `==`, `>=`, or `<=`.  This opens up MathematicalProgram to solve general nonlinear optimization problems."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[-0.70710678 -0.70710678]\n"
     ]
    }
   ],
   "source": [
    "mp = MathematicalProgram()\n",
    "x = mp.NewContinuousVariables(2, \"x\")\n",
    "mp.AddConstraint((x**2).sum() <= 1.0)\n",
    "mp.AddLinearCost(x.sum())\n",
    "result = Solve(mp)\n",
    "print(result.GetSolution(x))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Alternatively you can even use many numpy operations, including `dot`:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[-0.70710678 -0.70710678]\n"
     ]
    }
   ],
   "source": [
    "mp = MathematicalProgram()\n",
    "x = mp.NewContinuousVariables(2, \"x\")\n",
    "mp.AddConstraint(x.dot(x) <= 1.)\n",
    "mp.AddLinearCost(x.sum())\n",
    "result = Solve(mp)\n",
    "print(result.GetSolution(x))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Note that you can print out useful prints at many steps of interacting with Mathematical Program, for example:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "<class 'numpy.ndarray'>\n",
      "[Variable('x(0)', Continuous) Variable('x(1)', Continuous)]\n"
     ]
    }
   ],
   "source": [
    "mp = MathematicalProgram()\n",
    "x = mp.NewContinuousVariables(2, \"x\")\n",
    "print(type(x))\n",
    "print(x)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[<Expression \"pow(x(0), 2)\"> <Expression \"pow(x(1), 2)\">]\n"
     ]
    }
   ],
   "source": [
    "y = x**2\n",
    "print(y)"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.6.7"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
